function [v_new,w_new]=update_vl(v_before,v_before2,w_old,w_old2,c_before,p_ins,up_force,up_angle,ts)
%%
%用于速度更新
%输入：前一采样时刻速度v_before
%      前二采样时刻速度v_before2
%      前一时刻角速度w_old
%      前二时刻角速度w_old2
%      原始矩阵c_before
%      原始位置p_ins
%      加速度计输出up_force
%      陀螺仪输出up_angle
%      采样频率ts
%输出：新速度v_new
%      更新角速度
%%
% 参数引入
%地球参数设置
v_ie=7.292e-5; %地球自转角速率
a=6378254;     %地球长半径
e=1/298.3;     %偏心率
g=9.8;


%速度引入
v_e=v_ins(1);
v_n=v_ins(2);
%v_u=v_ins(3);

%位置引入
L_new=p_ins(1);
%la_new=p_ins(2);
h_new=p_ins(3);

%地球参数引入
R_m=a/sqrt(1-e^2*sin(L_new));
R_n=a*(1-e^2)/sqrt((1-e^2*sin(L_new)^2)^3);

R_mh=R_m+h_new;
R_nh=R_n+h_new;

%陀螺仪输出
angle_ts_begin=1/2*ts*up_angle;
angle_ts_half=1/2*ts*up_angle;
angle_ts_end=1/2*ts*up_angle;

%加速度计输出
vel_ts_begin=1/2*ts*up_force;
vel_ts_half=1/2*ts*up_force;
vel_ts_end=1/2*ts*up_force;
%%
%开始速度更新

%第一个矩阵
w_ie_new=[0;v_ie*cos(L_new);v_ie*sin(L_new)];
w_en_new=[-v_n/R_mh;v_e/R_nh;v_e/R_nh*tan(L_new)];

w_in_new=w_ie_new+w_en_new;

erv_in=ts*w_in_new;
erv_in2=1/2*erv_in;

c_n2=mrv(erv_in2);

w_new=[w_ie_new,w_en_new];%保存更新角速度
%第二个矩阵
up_vm1=vel_ts_half-vel_ts_begin;
up_vm2=vel_ts_end-vel_ts_half;
up_vm_sum=up_vm1+up_vm2;

up_m1=angle_ts_half-angle_ts_begin;
up_m2=angle_ts_end-angle_ts_half;
up_m_sum=up_m1+up_m2;

up_v_rotm=1/2*cross(up_m_sum,up_vm_sum);
up_v_scullm=2/3*(cross(up_m1,up_vm2)+cross(up_vm1,up_m2));

up_sf=up_vm_sum+up_v_rotm+up_v_scullm;

%第三个矩阵
w_ie_before=w_old(:,1);
w_en_before=w_old(:,2);

w_ie_before2=w_old2(:,1);
w_en_before2=w_old2(:,2);

w_ie_half=(3*w_ie_before-w_ie_before2)/2;
w_en_half=(3*w_en_before-w_en_before2)/2;
v_half=(3*v_before-v_before2)/2;
up_g=(g-cross((2*w_ie_half+w_en_half),v_half))*ts;

a_m=(c_n2*c_before*up_sf+up_g)/ts;

%速度更新方程
v_new=v_before+a_m*ts;
end